function Y = geodesic_generic_best(clusters, occupancy)

D = size(clusters.covars, 1) / 2;
Q = clusters.ncentres;
x = 1:D;
dx = (D+1):(D+D);

T = size(occupancy, 1);
I = eye(D);
Z = zeros(D);

%allocate space for mapping matrices
F1 = spalloc(3*D*(T+1) - 2*D, D*(T+1), 3*D*(T+1));
F1(1:2*D, 1:2*D) = [I Z; 
                    Z I];
for t=1:T-1
    rows = (t-1)*3*D + 2*D + (1:3*D); cols = (t-1)*D + (1:3*D);
    F1(rows, cols) = [I Z Z; 
                      Z I Z; 
                      Z Z I];
end
F1((T-1)*3*D + 2*D + (1:2*D), (T-1)*D + (1:2*D)) = [I Z; 
                                                    Z I];

for i = 1:clusters.ncentres
    K{i} = inv(clusters.covars(:, :, i));

    A{i} = computeA(K{i}, occupancy(:, i), x, dx, D, T);
    B{i} = computeB(K{i}, clusters.centres(i, x)', clusters.centres(i, dx)', occupancy(:, i), x, dx, D, T);

    if i == 1
        AA = A{i} * F1;
        BB = B{i};
    else
        AA = AA + A{i} * F1;
        BB = BB + B{i};
    end
end

Y = AA \ BB';
Y = reshape(Y, D, T+1)';



function A = computeA(K, gamma, x, dx, D, T)

Kxx = K(x, x);
Kxdx = K(x, dx);
Kdxx = K(dx, x);
Kdxdx = K(dx, dx);
A = spalloc(D*(T+1), 3*D*(T+1) - 2*D, 3*D*D*(T+1));
A(1:D, 1:2*D) = [ gamma(1) * Kdxdx, ...
                 -gamma(1) * (Kdxx + Kdxdx)];
for t =1:T-1
    rows = t*D + (1:D);     cols = (t-1)*3*D + 2*D + (1:3*D);
    A(rows, cols) = [-gamma(t) * (Kxdx + Kdxdx), ...
                      gamma(t) * (Kxx + Kxdx + Kdxx + Kdxdx) + gamma(t+1) * Kdxdx, ...
                     -gamma(t+1) * (Kdxx + Kdxdx)];
end
A(T*D + (1:D), (T-1)*3*D + 2*D + (1:2*D)) = [-gamma(end) * (Kxdx + Kdxdx), ...
                                              gamma(end) * (Kxx + Kxdx + Kdxx + Kdxdx)];


function B = computeB(K, mu, dmu, gamma, x, dx, D, T)

Kxx = K(x, x);
Kxdx = K(x, dx);
Kdxx = K(dx, x);
Kdxdx = K(dx, dx);
B(1:D) = - gamma(1) * (Kdxx * mu + Kdxdx * dmu);                                 
for t=1:T-1
    B(t*D+(1:D)) = gamma(t) * ((Kxx + Kdxx) * mu + (Kxdx + Kdxdx) * dmu) - gamma(t+1) * (Kdxx * mu + Kdxdx * dmu);    
end
B(T*D+(1:D)) = gamma(end) * ((Kxx + Kdxx) * mu + (Kxdx + Kdxdx) * dmu);  
